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I.  INTRODUCTION 


Robust  filtering  and  smoothing  are  a  natural  extension  of  the  robust 
M-estimates  of  regression  developed  by  Huber  £l].  The  robust  M-estimates 
provide  a  natural  treatment  of  outlying  observations  and  have  been  extremely 
successful  in  dealing  with  outliers  in  other  data  reduction  problems  [2] 
and  [3].  The  extension  of  the  M-estimates  of  regression  to  filtering  and 
smoothing  provides  a  fresh  approach  to  the  problems  caused  by  outliers  in 
filtering  and  smoothing  applications.  Robust  methods  for  estimation  have 
been  designed  to  perform  well  when  observations  from  contaminating  distri¬ 
butions  are  present.  The  conventional  estimation  techniques  of  least  squares, 
minimum  variance,  maximum  likelihood,  etc.  may  become  useless  when  the  ob¬ 
servations  are  contaminated  by  outliers.  In  filtering  and  smoothing  appli¬ 
cations,  outliers  have  often  been  treated  by  testing  the  filter  or  smoother 
residuals.  If  the  residual  is  too  large  relative  to  some  measure  of  dis¬ 
persion  of  the  residuals,  the  corresponding  observation  was  rejected  and 
considered  to  have  been  obtained  from  a  contaminating  distribution.  Other¬ 
wise,  the  observation  is  processed  normally  by  the  filter  or  smoother.  This 
procedure  was  often  successful  when  only  a  small  proportion  of  outliers  were 
present  in  the  observation  sequence.  Also,  in  order  that  such  an  outlier 
detection  sequence  be  successful,  a  robust  measure o f  dispersion  of  the  filter 
or  smoother  residuals  is  necessary.  These  old  methods  of  treating  outliers 
in  filter  or  smoother  observations  were  added  to  the  filter  or  smoother  algo¬ 
rithm  as  an  afterthought.  In  contrast  to  this  the  development  of  robust  filter 
ing  and  smoothing  methods  by  the  use  of  M-estimates  provides  a  method  of  treat¬ 
ing  outlying  observations  which  is  inherent  in  the  filter  or  smoother  equations 
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In  previous  reports  [4]  and  [5]  we  have  developed  the  extensions  of 
M-estimates  using  a  specified  p  function  to  the  filtering  and  smoothing  of 
tracking  data.  This  application  was  evaluated  extensively  using  Monte  Carlo 
methods.  The  results  of  this  evaluation  showed  a  significant  decrease  in 
estimation  error  using  Hampel  p  functions  when  small  outlying  observations 
were  present.  When  large  outliers  were  present,  filters  using  M-estimates 
offer  complete  protection  against  the  destructive  effect  of  these  outliers. 
Also,  when  no  outliers  were  present,  there  was  negligible  loss  in  efficiency 
using  filters  with  M-estimates  as  compared  to  using  an  ordinary  Kalman  fil¬ 
ter. 

The  robust  filtering  and  smoothing  techniques  developed  in  this  report 
are  an  application  of  the  work  of  Masreliez  [6]  on  approximate  non-Gaussian 
filtering.  We  assume  that  the  filter  or  smoother  observations  are  sampled 
from  a  Gaussian  mixture  pseudo-density.  After  deriving  the  robust  filter¬ 
ing  and  smoothing  equation,  we  give  the  results  of  an  extensive  Monte  Carlo 
evaluation  of  these  robust  techniques  when  applied  to  simulated  tracking 
data  in  the  presence  of  measurement  noise  contaminated  by  outliers.  We 
also  compare  these  Monte  Carlo  results  with  equivalent  Monte  Carlo  results 
obtained  using  robust  filters  based  on  Hampel  p  functions. 


II.  APPROXIMATE  NON-GAUSS IAN  FILTERING 


Assume  that  the  state  of  a  process  is  given  by  the  linear  model 

x(k+l)  *  s(k+l,k)x(k)  +  u(k),  (1) 

where  the  state  vector  x(k)  of  the  process  is  an  m-vector,  u(k)  is  a 
Gaussian  state  noise  vector  with  zero  mean  and  covariance  Q(k).  $(k+l,k) 

is  an  mxm  transition  matrix.  Let  scalar  observations  of  the  process  be 
given  by 

Z(k)  =  HCk)x(k)  +  V(k),  (2) 

where  H(k)  is  a  row  vector  and  V(k)  is  a  measurement  noise  error  which  may 
be  contaminated  by  outliers. 

In  many  situations  the  conditional  mean  provides  an  optimal  estimate 
of  a  parameter  or  process.  We  will  follow  the  work  of  Masreliez  [6]  in 
deriving  an  approximate  conditional  mean  of  the  process  specified  by  (1) 
and  observed  by  (2).  We  denote  the  conditional  mean  conditioned  on  the 
observations  in  the  set  Zk  =  {Z(l } ,Z(2) - ,Z(k) }  by  x(k|k)  =  E[x(k)|Zk]. 

Let  p(x(k)|Zk)  be  the  probability  density  of  the  state  x(k)  conditioned 

k  k 

on  the  observation  set  Z  .  Using  Bayes  rule  p(x(k)|Z  )  can  be  written  as 

ptxWizb- 

p(Z(k) |Zk*  ) 

The  basic  assumption  in  the  derivation  of  Masreliez  is  that  p(x(k)|Zk_1) 
is  Gaussian  with  mean  x(kjk-l)  and  covariance  P(k|k-1).  The  conditional 
mean  estimate  of  x(k)  is  given  by 

JCklk)  >  E[x(k)|zk]  .  ALklpCxtkiiz^jpCztkiixCkMdxCk) 

J  p(zCk)!zk'’)  w 
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Subtracting  x(k'k-l)  from.  both  side  of  {V 

, .  '  •  \  _  y.  f  •  1  1 .  7  \  “  l  t  -  /  *  '  -  k  ^  ^  [  f  t  \  /  ,  r  *  \  t  r  f  ,  »  -T  .<  -  i  \ 

-X  -  3  >  '  j  ^Xv<  .<-  1  }  ;pLx,.<i  ;i  ) 

*/  p  ( ZC < )  ’  :<(.<■) }  dx  ( k ) 

Using  the  Gaussian  assumption  for  p(x(k) '  Zk”  1 ) ,  i.e.,  ?(x(k)  :  Z<_  1 )  = 


M C X C < ) _ :< C !<  k-I ;  ,  ?(k; we  can  writ: 


(x(k)-x(k.k-l))p(x(:<)  ;zk_1 )  =  -?(k;k-i)-^)?(x(k; 


‘  7k~1' 


Using  (5)  in  (5)  results  in 


x(k; k)  =  i  (!< !  k- 1 }  -  ?" 1  (Z  (k }  i  Zk"  5P  ( 1 ) 


p(Z(k) ■ x ( k ) )dx(k) 


Integrating  (7)  by  parts  gives, 


x(k|k)  =  x (<  j  k-1 )  +  p'1 (Z(k) i Zk_1 )P(k; k-1 )  r  p(x(k)!Zk-1)T7|7)P(Z(k)!x(k))dx(k) 

/  .  ’  '  (3) 


Using  p(Z(k)  ;X'k) )  =  -HT(k)T|lF}?(Z(k)!x(k)) 

in  (8)  we  obtain 


x (:<;•<)  =  xik:k-i)-p'1(z'k);zk'1)p(k;k-])HT(k)T47T  f pCZ(:<) -z^1  }o(ZCk) ixCk) 7-dx 

» v.  <  /  I  /in' 

j  * 


Using  p Cx c< : ; Zk_ 1 } p CZ Ck } ; X Ck)  «  p(x(k),z(k);zk'1),  (10)  b 


becomes 


x(kik)  »  xCk'  k-l)  +  P(k,  k-1  )H.‘  (k)g(Z(k) ) , 


where  g(Z(k})  is  the  scalar 


n  !  7  '  7  M  7^  ''_l_  7<-|\ 

J  v-  v<  1  -  ;  :  -  ■  ■  , 

-^t</ 


Cl  2) 


(11 )  and  (12)  were  derived  by  Masrel  isz  in  f5]  for  approximat  inc  the  nini- 
mum  variance  filter  when  the  measurement  noise  is  r.on-Saussian.  In  orcer 
to  complete  the  specification  of  this  approximate  filter,  it  is  necessary 
to  cerive  an  expression  tor  the  conditional  second  moment, 


’('<)  =  £  i  (x(k)-x(k; k) )  (x(k)-x ( k f  '< ) )  ’  ,2 


<  i 


03] 


An  expression  for  computing  ?(k)  is  derived  similar  to  the  derivati 


ion  or 


x(kjk).  This  derivation  is  outlined  below. 


POO  =  E  [ (x(k)-x(k; k-1 ) ) (x(k)-x(k] <-l ) )T ]2kj 

.  -  (x  ( k  j  k )  - x  ( k  |  !<- 1 ) )  ( x  ( !<  •  !< }  -x  ( k !  k - 1 ) ) 1 
Let  S(k)  =  Ej  (x(k)-x(k ‘ k-1 ) ) (x(k)-x(k j  k-1 ) )T ; Z^] . 


04) 


'hen  using  (3) 


SOO  =  P*"1  U(0  j Z'<_^ )  /  (x(k)-x(k i  k-1 ) )  (x(k)-x(k!  k-1 )  )Tp(x(k) ;  z'<_1  )p(2(k)  ] x(k)  )cx (k) 

J  *  05) 

,  ■  k_l 

Assuming  P',x(k)  Z  )  is  Gaussian,  we  use  (6)  and  integrate  by  parts  twice 
to  obtain. 


SCO  =  ?( 


(k  k-1)  a-  ?('<!k-l'Hr'k)  /  o" 7  k  ~ ]  1  iz  li-Ujj} .  -Z  *  2  il  n  (\,  1  o  r*  l  u  .  1 1 

l  / . 05) 


Combining  (15)  with  (;-)  and  (11;  gives, 


=  (k.k)  =  P(k  k-1)  -  ?(:<  k-1  )H‘  (k)G(Z(k) )H(k;?(k  k-1), 


wnere 


}c(Z'V ) 

i  l  i  k  i 


(17) 


(-3) 


3(Z(k) ) 


III.  THE  ROBUST  CONDITIONAL  MEAN  FILTER 


We  apply  (11),  (12),  (17),  and  (.18)  to  derive  a  robust  filter.  We  as¬ 
sume  that  p(Z(k)  |x(k))  is  the  Gaussian  mixture, 


P(Z(k)  jx(k) )  =  za.jN(Z(k)-H(k)x(k)-a[i),Rk) 


(19) 


In  (19) 


N(Z(k)-H(k)x(k)-a[1‘),Rk)  =f/^)EXpj-(Z(k)-H(k)x(k)  -a^1  h2/*\  j  C20) 

We  do  not  require  that  la.  =  1  so  that  (19)  may  not  be  a  density  function, 
but  rather  a  pseudo-density.  Also,  the  sum  in  (19)  may  be  infinite.  Thus, 
we  have  individual  Gaussians  centered  at  aj^,  each  having  standard  deviation 

/T^-.  The  locations  a^  and  the  amplitudes  ai  are  considered  design  para- 

k- 1 

meters  of  the  robust  filter.  We  obtain  p(Z(k)jZ  )  from 


p(Z(k)jZk’1)  =  rp(x(k)|Zk-1)p(Z(k)!x(k))dx(k)  (21) 

•k 

k- 1 

Using  (19)  and  the  Gaussian  assumption  for  p(x(k)|Z  )  the  convolution  in 

(21)  gives  p(Z(k)  | Zk_1;  =  za  p(Z(k)-H(k)x(k| k-1  )-a^ } ,  M(k))  (22) 

i  i  K 

where  M(k)  is  the  covariance  of  the  residuals, 

M(k)  =  H(k)P (k ! k- 1 )HT(k)  +  (23) 

Using  (22)  in  (11)  and  (12)  we  obtain  the  filter  equation, 

x(k|k)  =  x(kjk-l)  +  P(k|k-l)HT(k)M'1(k)(Z(k)-H(k)x(kjk-l)-a'k),  (24) 


where 


ak  =  ™ 
i  1  K 


(25) 


o 


The  weights  'A ■  are  given  by 

w,  =  -1— - - - -  (25) 

I-.N(Z(k)-H(k)x (k ;  k-1  )-aM  ,M(k) ) 

-.*  J  ^ 

J 

In  cowing  she  weighted  average  in  (25)  it  is  not  necessary  to  compute 
ail  the  terms  in  the  sum,  since  many  of  the  weights  will  be  zero  -'or  ail 
practical  purposes.  We  only  need  to  compute  those  terms  in  (25)  and  the 
corresponding  weights  in  (25)  for  wnich  |Z(k)  -  H(k)x(k  k-1 ) -af 1  ^ -3/M^7)\ 

This  consideraoiy  simplifies  the  computation  of  the  robust  filter.  The  co- 
variance  of  the  residuals,  M(k),  is  estimated  from  past  predicted  residuals 
using  the  robust  MAD  estimate, 


/NKk)  =  median  I  Z(k-j }-H(k-j )x(k-j ! k-j-1 )  j  / . 5745  (27) 

j=0,M-l  !  |  / 

The  conditional  covariance,  ?(k),  is  obtained  using  (22)  in  (17)  and  (13), 

T  /  -1  -2  .  2 \  T 

?(k)  =  ?(k:;<-i)-P(:<,k-l)Hl  (k)  ^  M(k)-N(k)(aJ-ak)  (k)P(kjk-l ) ,  (28) 


where 


lak'!-ak>  *  fi 


Only  those  terms  in  (29)  for  which  JZ(k)  -  H ('<);<(’< )  k-1 )  -  a^')l  -  3*MCk ') 


are  ccmcuted. 


The  locations,  a^,  produce  a  smooth  pseudo-density  if  they  are  chosen 

at  zero  and  odd  intecral  multiples  of  »M(< '•  ,  i.a  ,  a^  =  0  a^1'  = 

K  ’  k 

sen  (i )(  2 ; i ; -1)  /M(k) ,  i  -  1.  We  have  also  tasted  the  filter  with 


a,^1 '  =  i  •  *to(k)  ,  |ij  -  0.  Several  different  choices  of  the  amplitude  have 
been  tested.  The  most  extensive  testing  has  been  done  with  s.  =  1  and  cu  = 


IV.  MONTE  CARLO  EVALUATION  OF  THE  ROBUST  FILTER 


Evaluation  of  the  robust  filtering  method  described  here  has  been  done 
with  a  view  toward  eventual  application  to  trajectory  estimation.  Emphasis 
in  the  evaluation  is  on  the  use  of  simulated  rather  than  real  trajectory 
data.  This  allows  a  quantitative  determination  of  any  advantages  in  the 
use  of  robust  filtering  in  the  presence  of  outliers  and  also  any  loss  in 
efficiency  using  robust  methods  when  no  outliers  are  present.  The  simulated 
trajectory  is  that  of  a  constant  velocity,  level  flying  aircraft.  The  filter 
model  assumes  the  trajectory  to  have  constant  acceleration  in  three  cartesian 
coordinates.  Let  x,  y,  z  be  the  East,  North,  and  Up  components  of  trajectory 
position.  We  assume  that  the  dynamic  model  for  each  of  the  coordinates  is 
given  by 


x-|  (k+1 ) 

"l  A  a2/2~ 

x-|  (k) 

~  0 

x2(k+l) 

= 

0  1  A 

x2(k) 

+ 

0 

x3(k+l) 

0  0  1 

J 

x3(k) 

, 

W(k) 

where  A  =  Vi  "  V  x ^ (k) ,  x2(k),  Xj(k)  are  position,  velocity, 


(30) 


and  ac¬ 


celeration  components,  respectively.  W(k)  is  a  zero  mean  Gaussian  accelera¬ 
tion  state  noise  with  variance  q.  The  filter  observations,  Z(k),  are  scalar 
positions  corrupted  by  additive  noise, 

Z(k)  =  H(k)x(k)  +  V(k) , 


with  H(k)  =[10  0].  The  measurement  noise  is  Gaussian  with  covariance 
R(k).  The  measurement  noise  is  contaminated  by  outliers  which  are  generated 
by  choosing  the  mean,  y(k),  of  the  measurement  noise 

0  if  no  outlier 


u(k)  = 


u-|(k)  if  outlier  present 


(31) 


In  order  to  decide  whether  or  not  an  outlier  is  to  be  present  at  each  time 
t^,  we  use  a  two  state  Markov  chain.  Let  i  denote  the  state  of  the  Markov 
chain,  i  =  1  is  the  state  of  no  outlier  present  and  i  =  2  is  the  state  of 
having  an  outlier  present  in  the  data.  Let  Pjj(k)  he  the  probability  of 
a  transition  from  state  i  to  state  j  in  the  interval  (t^  ,  t^}.  The  transi¬ 
tion  probabilities  are  chosen  to  provide  a  given  percentage  of  outliers  in 
the  observations  and  also  to  generate  a  desired  average  run  length  of  out¬ 
liers.  The  transitions  between  states  are  realized  by  use  of  a  pseudo  random 
number  generator. 

The  constant  velocity  trajectory  used  for  evaluation  is  given  by 

=  x(tk)  +  x(tk+rtk) 

*(1W  =  y(tk}  +  y(tk+rV  (32) 

z(tk+1)  -  z(tk)  ♦  z(tk+rtk) 

•  •  • 

with  x  =  -550  ft/sec,  y  =  -525  ft/sec,  and  1  =  0.  A  sampling  interval  of 

tk+i  =  .05  sec  was  used.  A  Monte  Carlo  evaluation  of  the  filter  is  done 

by  computing  seme  statistics  of  the  filtering  errors  over  N  filter  runs. 

Let  x^(tk),  y.j(tk),  and  Z ^ ( t ^ )  denote  the  filtered  position  estimates  at 

time  tk  for  the  i—  run  and  let  x.(tk)  =  x^t^)  -  xi(tk)>  y^Ct^)  = 

-  -v 

yi  (tk)  -  y1(tk),  and  Zi  ( tfc)  =  Z^t^  -  Zj  (tk)  denote  the  error  in  filtered 

f  t. 

positions  for  the  i—  filter  run  at  time  tk.  Also,  let  x,Ctk)  e 
.  'v  J 

#  0  0  •  •  • 

x  ‘  xi(tk),  yi(tk)  *  y  -  yi (tk) ,  and  Z1(tk)  =  Z  -  ZjCt^}  denote  the  errors 
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ne  xSi  pcsivon,  /e iccity,  anc  acceleration 


e$  of  tf.e  3SS  error  defined  in  (33). 
N 


N 


i  =  l 


(33) 


(34) 


N  " 

i=I  1  * 

:aticn  af  the  robust  filter  to  the  comparison 


imoute  the  time  averaoe  of  the  estimation  errors 


wr.ere  A  is 
tied,  N  = 


;erwise  5 


ition  prqfcaoi  > ’ ti ss  ro r  t.ne  .mar<cv  c.na'n  provide  an  cu ~  : er  ccntami na *•  or 
length  of  three.  A  state  noise  variance,  q  =  5,  was  used  *or  all  filter 
runs.  In  all  »Mjns  of  the  robust  filter,  the  measurement  noise  variance 
ROO ♦  "as  unknown  to  t.ne  filter.  The  residual  variance,  which  is  the  or.1/ 
quantity  involving  R ' < ana  required  by  the  filter  was  estimated  us'ng  the 
MAD  estimate  of  [27). 

Figures  1  and  2  present  the  average  RSS  position  and  velocity  filter¬ 
ing  errors  ror  a  Gaussian  mixture  robust  filter  with  observations  contamin¬ 
ated  by  various  magnitudes  of  outliers.  The  Gaussian  mixture  filters  used 
in  generating  Figs  1  and  2  used  magnitudes  of  the  Gauss ians,  -  1/(H'  +  1) 

Two  different  Gaussian  mixture  filters  are  represented  in  Figs  1  and  2,  one 
with  Gauss  Tans  at  ail  integral  multiples  o*  the  residual  standard  deviation 
Sj<»  2nd  one  with  Gaussians  at  zero  and  odd  integral  multiples  of  Sfc.  Each 

component  of  the  Gaussian,  mixture  representing  ?(Z,  :Zk'1)  has  standard  de- 

viation,  S, . 
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There  is  ve^y  little  difference  in  the  estimation  errors  c f  the  two  filters 
presented  in  F^gs  1  and  2.  The  filter  with  Gaussia.ns  at  only  odd  r.ul  titles 
of  Sk  is  somewhat  less  ccnputa t i cnal  1y  complex  so  that  it  might  be  considered 

the  preferred  filter  of  the  two  filters  presented  in  Figs  1  and  2.  Figs  3 
and  4  compare  the  a/erage  estimation  errors  for  two  Gaussian  mixture  filters 
which  have  Gaussia.ns  at  zero  and  ode  integer  multiples  c*  5^.  One  o'  these 

filters  uses  Gaussian  amplitudes,  ^  =  1/1'i'  1)  and  the  other  uses  equal 

a  .  =  1 , 

1 


amplitudes  for  the  Gaussians,  i.e., 


s 


:tc 


ji  - 

ac 


X  X 


Jp  4  i  —  — 


A  CM 

X  Hal  1  ,2 ,3i 


X 


41ft  3  Hr"  12  HR 

Out!  i ?r  L v v e  1 


loNR 


The  Gaussian  mixture  filter  in  Figs  5  and  6  gives  somewhat  smaller  esti¬ 
mation  errors  than  the  robust  filter  using  Ha(l,  2,  3). 

The  robust  Gaussian  mixture  filter  was  also  evaluated  with  respect  to 
its  ability  to  adapt  to  changes  in  the  measurement  noise  variance.  Using 
the  same  simulated  trajectory  as  before,,  the  measurement  noise  standard 

deviation  was  taken  to  be  /R ( k )  -  20  ft  for  0  -  t  -  25  sec,  * R  ( k )  =  100  ft 

for  25  <  t  -  40  sec,  and  /R ( k )  =  50  ft  for  40  <  t  -  50  sec.  Figs.  7  and 
8  compare  the  average  estimation  errors  of  the  Gaussian  mixture  filter  with 
=  1  and  having  Gaussians  at  zero  and  odd  integer  multiples  of  St<  with 

the  estimation  errors  of  the  robust  filters  using  the  Hampel  v  functions 
Ha ( 1 ,  2,  3)  and  Ha  (3,  3,3)  under  the  above  measurement  noise  variations. 

The  results  for  Ha  (1,  2,  3)  and  Ha  (3,  3,  3)  were  reported  in  [4j.  Except 
for  the  way  in  which  the  dispersion  estimate,  S^,  of  the  filter  residuals  is 

estimated,  the  filter  using  Ha  (3,  3,  3)  represents  a  conventional  way  of 
handling  outliers  in  a  Kalman  filtering  application.  Figs.  7  and  3  show 
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hat  both  the  Gaussian  mixture  rcoust  n  iter  and  toe  robust  ri;ter  Ha  'w< » 

)  have  considerably  smaller  estimation  errors  than  the  more  conventional 
liter,  Ha  (3,  2,  3),  for  this  application.  Figs.  7  and  3  also  indicate 
hat  the  >-ooust  Gaussian  mixture  filter  has  somewhat  smaller  estimation  er 
ors  than  the  robust  filter  using  the  Hampel  v  function,  Ha  (1,  2,  3). 
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V. 


APPROXIMATE  MCM-3AUS5 IAN  SMOOTHING 


In  the  following,  seme  rooust  fixed  lag  smooching  equations  are  de¬ 
rived  in  a  way  similar  to  the  derivations  of  the  robust,  Gaussian  mixture 
filter  equations,  i.a.,  using  the  conditional  mean  derivation  employed  by 
Masreliez  and  assuming  a  Gaussian  mixture  pseucc  density  -or  the  measure¬ 
ment  noise.  In  fixed  lag  smoothing  an  estimate  of  the  state  x(k)  of  the 
system  described  by  (1)  and  (2)  is  aesired  using  the  measurements  Z(l), 
1(2),  — ,  Z(k),  — ,  Z  ( k+N ) .  Let  dZk+N  =  { Z  ( !< ) ,  Z(k+1),  Z(k+2),  — , 
Z(k+N)}.  Then  Zk'r,'i  =  Z<-"'  The  posterior  conditional  density  is 


given  by 

P(x(k)iZk+NJ  « 
k- 1 

We  again  assume  that  P(x(!<) \t  )  is  Gaussian.  The  conditional  mean, 
x(kjk+N)  =  E[x(k)|Zk+N]  is  given  by 


aZk+,N!x( 


P(iZk+l'J  I  Zk""'l 


(36) 


x  C  k  |  ’<+ 


=  P‘1(aZk+NjZk‘1^y’x(k)P(aZk+N!x(k))P(x(k) 


)dx(k) 

(37) 


Adding  and  subtracting  x(kjk-l)  gives 


(kjk+M)  =  x(k|k-l)  +  P*1UZfcfM|Zk‘1)/  (x(k)-x(k!k-I))P(x(k)jZk'1) 


P(iZk+N|x(k))dx(k) 


Assuming  P(x (k) j Zk_1 )  is  Gaussian,  we  use  (5)  to  obtain 

xCkik+M)  =  x(klk-l)  -  P“](iZk+?l|Zk“1)PCk|k-l)  r  P(x(k)lZk_1) 

?CiZk'N!x(k))dx(k)  J  , 
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Intagracing  (39)  by  oarts, 


x(k:  <+N)  »  x(k;k-l)  *  P'^iz'^iZ^1  f?  (x(k)  Zk_1]  -f-, 

Jn  m  ‘ 


PUZk+N;xCk))dxC<) 


C^o) 


Assuming  statistical  independence  o*  the  observations 


i  nen 


where 


P(AZk+Njx(k))  =  n  P(Z(k+j ) 'x(k) } 
j=0 


n 


Q,-  =  n  p(z(k+j !x(k)) 
J  Irj 


Assuming  no  scata  noise  in  the  forward  interval,  we  have 


(41) 


(42) 

(43) 


jPiZi^jrx(kJ),  =  T,  )HT(  }  lP(Z(k-i):x(k)) 

3x(k)  ;  ^  k+j’  V  {K'3)  sZ(.k+j ) 

Substituting  (41)  -  (44)  into  (40)  gives 

N 

i ( k  i  k h'O  =  x  ( k ;  k- 1 ) -p ' 1  ( az k+N ; x  (k ) ) p ( k ;  k- 1 )  z  f T  ( tk+ j ,  t k ) ht ( -<- j ) 


j=0 


3Z(kfj ) 


>(-:k*N'x(k))P(;<(k);Zk'1!<ix('i<) 


P- 

J  R'71 


(44) 


(45) 


p(iZkt"ixU))p(x(ki!Zk-')  .  Puzk+N,x(k);zk-') 
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3ut 


Then 

x(k'k-i-N) 


x (:<':<-!) -o' 'uzk"N  zk',!?(:<;k-i)  '  */(•  :  ) 

j=0  '<J  < 


iirkr  p^zk+N:zk'!) 


In  order  to  use  (46)  for  rocust  smoot.ning,  it  is  necessary  tc 
k+M •  k- I 

specify  p(aZ  '  |Z  )  so  that  it  has  heavy  tails  and  can  be  used  in 

(46)  to  arrive  at  a  useful  robust  smoother  without  too  much  complexity. 

p(iZ  |Z  )  can  either  be  specified  directly  cr  computed  via 

integration  from  a  specification  of  p(aZ<+N |x(k- I ) ) .  In  the  latter 

approach  the  integration  is  too  difficult  when  trying  to  achieve 

k^N 

robustness  by  specifying  that  p ( _Z  '  x ( < - f ) )  =  “p(Z(k+j)  ix(k- I ) ) 

j 

and  assuming  that  p(Z('<J-j)  'x(k-l ))  is  a  Gaussian  mixture.  In  the 

former  approach  assuming  pCtZ^  ‘Zk~  1 )  =  np(Z(k+j)  jZk'  * ) 

j 


with  p(Z(k+j )  |Zk" ' )  a  Gaussian  mixture  is  easy 

to  handle  mathematically  but  results  in  poor  smoothing  performance. 

This  poor  performance  is  probably  due  to  the  fact  that  the  variables 

k- 1 

Z(k+j)  conditioned  on  Z  are  not  independent.  For  lack  of  a  good 
method  for  specifying  p( cZk ' )  in  (46)  we  abandon  the  conditional 


mean  approach  to  rocust  smoothing. 
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A  useful  rooust 


A  useful  rooust,  fixed  lag  smootne-'  is  cotsined  *  * ,  'Instead  o~ 
finding  the  conditional  mean  of  p(x(k)  ,Z^  ,  we  compute  the  ncce  o* 

?(.<(!< )  [  ZK  '^1 .  This  estimate  maximizes  '36)  or  equivalently  minimizes 

l(x(k)}  =  -  log  pCiZ^ixCO)  -  log  p(x(k)  :Z<  *)  (47' 

k  i 

Assuming  that  p{x£<)  (I  )  is  Gaussian  with  mean  x'kk-l)  and  covariance 
P  ( k ! k - 1 ) ,  (47)  is  equivalent  to, 

(43: 

L ( x ( k ) )  =  (x(k) -x (k ; k- 1  )T?'  1  (k  j  <-  I )  (x(k)-x(k ; k-  I ) )- 1  og  x(k) ) 


Minimizing  (43)  by  setting  =  0  gives 


xC<>k+N)  =  x ( k i k - 1 )  +  ?(kik-l)p'i(AZk"Njx(kik+N)) 


Assuming  that 


:-p(aZk+Njx(k;k-N)) 
3x(k  J  k-*-N) 


o(aZ:<‘r‘1!x(k))  =  n  p ( z ( k-j  ) x ( k ) ) 
j=0 


jo(  x  (k) )  11  3p(Z(k+j )  i x ( k ) ! 

-  =  )  Q,  - 

3 x ( k )  .  n  J  3x(k) 


wnere 


Q.  =  7.  p(Z(k-i )  |  x  C'< ) ) 

J  i  r: 
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Also,  we  can  write 


3p(Z(k+j) | x(k) }  ,  T  3p(Z(k+j ) | x ( k ) ) 

SxTkl  =  3Z(k+j) 


(53) 


Then  using  (50)  -  (53)  in  (49) 

N 


x ( k i k+N )  =  x(k|k-l)-P(kik-l)  l  *  (t.  .  ,t,  )H  (k+j ) p~  (Z(k+j ) |x(k | k+N) ) 

j=0  *  J  *■ 


(54) 


3p(Z(k+j ) ] x ( k | k+N ) 
3Z(k+j) 


Since  x(k|k+N)  appears  nonlinearly  on  the  right  hand  side  of  (54),  the 
solution  of  (54)  will  usually  require  iteration..  We  call  the  expression 
for  robust  smoothing  given  in  (54)  the  MAP  formulation  of  robust,  fixed 
lag  smoothing. 


2 


I 


/: .  ROsys~  vap  smoothing  via  gaussian  fixtures 

In  order  to  ootain  a  robust,  fixed  lag  smoother  via  tne  MA P  formu  I  ation, 
we  reoiace  the  tensities,  p(I(<i-j )  ,x(’<) ) ,  by  the  Gaussian  mixture  pseudo- 
densities 

p ( Z ( !<+ j ) ; x ( k) )  =  “  » . -N( Z ( k-j } -H ( k+j } ?( t,  . , t.  ) x ( k ) -a5 1 J ,  R(k+j))  (55) 

1  *■  J  X  X  J 


In  (55)  we  have  individual  Gaussians  centered  at  a^+j  having  variance 

R(k+j).  The  amplitude  of  each  individual  Gaussian  is  specified  by  a ..  Then 

(56) 

.  3p(Z(k+j  )  j  x  ( k ))  *R'l(k+j)[  af(Z(k+j)-H(k+j)»(tk+j,tk}x(k)-a^]  )N 

p  l(Z(k-^j)|x(k)) - o— - - =  - 5 - — - -  J  1 


3Z(k+j) 


1 *iNi 


where  ^  =  M(Z(k+j  ) -H(k+j  )  ,tk)x(k.)-a,^j  ,R(k+j ) ) 


(56)  can  be  written  as 


p"  (Z(k+j  ) ;  :<(k) ) - 


3p(Z(k+j ) j  x(k) ) 


=  -R"  *  (k+j  )(Z(l<+j)-H(k+j  )  >(tk+j  ,tk)x(k)-a,<+J 


where  a,  is  the  weighted  averaae  of  the  aM), 


1..  =  T  wfl?  af1! 
k-^j  r  k+j  k*j 
i 


r 


4 


where  the  weights  satisfy  £  Vlj'jj  =  I  and 


,0)  = 


iNi(Z(k+j)-H(k+j)$(tk+j  ,tlc)x(k)-a([]:j,R(k+j)) 


I  aiNi (Z(k+j )-H(k+j )«(tk+j ,tk)x(k)-a[|] ,R(k+j ) ) 


(59) 


Substituting  (57)  into  (54)  gives 


x(k jk+N)  =  x(k|k-l)+P(k|k-l)  ^  $T(tk+j ,tk)HT(k+j)R‘' (k+j) 


(Z(k+j)-H(k+j)<f(tk+j  ,tk)x(kJk+N)-ak+j) 


(60) 


s 

r 

i 


Li 


Rearranging  (60)  results  in 


N 


x(k|k+N)  =  x(k|k-l)+P(k)  ^  *T(tk+j ,tk)HT(k+j)R_l (k+j) 


(Z(k+j)-H(k+j)*(tk+j,tk)x(k|k-l)-ak+j), 


where 


(61) 


P" ' (k)  *  P“'(k|k-I)  +  j  ♦T(tk+j,tk)HT(k+j)R"l(k+j)H(k+j)«(tk+j,t|()  (62) 

j  ~0 

Note  that  ak+J.  is  also  dependent  on  x(k|k+N)  so  that  it  is  necessary  to 
iterate  (61)  in  order  to  obtain  a  solution. 
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! 

J 


In  computing  the  weighted  averages,  a.  , ■  ,  it  is  not  necessary  to  compute 

IwJ 

all  the  terms  in  the  sum  since  many  of  the  weights  will  be  zero  for  all 
practical  purposes.  We  only  compute  those  terms  for  which 
|Z(k+j)-H(k+j)*(tk+j,tk)x^  (k|k+N)-akjj |<  3/M^  . ,  where  a  is  the  iteration 
index  and  xv  ;{k)k+N)  is  the  approximation  to  x(kjk+N)  at  the  ath  interation 
step,  M(k+j)  is  a  robust  measure  of  the  variance  of  the  residual, 
Z(k+j)-H(k+j)$(tk+j,tjJx(k|k).  This  considerably  simplifies  the  computation 
of  the  ak+j  since  only  a  few  of  the  weights,  w^j,  given  by  (59)  need  to  be 
computed. 

It  is  also  necessary  to  have  a  robust  estimate  of  the  observation  noise 
variance,  Rk+j  .  to  be  used  in  the  smoother  in  (61)  and  (62).  We  have  tried 
several  ways  of  obtaining  a  robust  R(k+j)  and  have  found  that  the  use  of 
R(k+j)  =  M(k)  where  M(k)  is  computed  from  the  past  filtered  residuals  by 
(27)  worked  best  in  our  evaluations.  We  form  a  robust  measure  of  M(k+j)  by 


M(k+j )  =  H(k+j)f(tk+j,tk)P(k|k)«T(t)t+j  ,tk)  +  R(k+j) 


(63) 


As  in  the  robust  filter  we  have  taken  equal  amplitudes  for  each  term 
in  the  Gaussian  sum,  i.e.,  =  I  and  have  found  that  taking  the  locations 


ak!j  at  0  and  odd  integral  multiples  of  /M(k+j )  again  works  well. 


VII.  EVALUATION  OF  GAUSSIAN  MIXTURE  RQ8UST  SMOOTHING 


A  Monte  Carlo  evaluation  of  the  robust,  Gaussian  mixture  smoother  was 
performed  using  the  same  simulated  trajectory  as  used  for  the  robust  filter 
evaluation.  A  smoothing  interval  of  I  sec.  or  20  points  was  used.  The 
Monte  Carlo  sample  size  for  the  smoother  evaluation  is  N  =  10.  The  measurement 
noise  standard  deviation  was  increased  so  that  /R(k)  =  50  ft.  The  outlier 
contamination  is  the  same  as  for  the  filter  evaluation,  i.e.,  a  outlier 
contamination  of  8.8"  and  an  average  outlier  run  length  of  three.  The 
evaluation  was  performed  using  three  iterations  of  the  Gaussian  mixture 
MAP  smoother. 

Figs  9  and  10  compare  the  average  RSS  position  and  velocity  errors  for 
a  Gaussian  mixture  MAP  smoother  contaminated  by  various  magnitudes  of 
outliers  with  the  corresponding  average  RSS  errors  of  a  robust  MAP  smoother 
using  M-estimates  with  a  Hampel  ^-functions  which  we  denote  by  Ha(3,3,3) 
and  Ha( 1,2,3).  The  robust  MAP  smoother  using  Ha(3,3,3)  and  Ha(l,2,3)  was 
described  in  [4].  Also,  plotted  in  Figs  9  and  10  are  the  ideal  values  of  the 
RSS  errors  which  are  obtained  with  an  ordinary  optimal  smoother  when  no 
outliers  are  present  and  the  noise  variance  is  known.  Figs  9  and  10  indicate 
that  the  Gaussian  mixture  robust  smoother  has  some  loss  of  efficiency 
(at  least  at  the  position  level)  when  no  outliers  are  present.  Figs  9  and  10 
indicate  that  the  Gaussian  mixture  robust  smoother  has  somewhat  smaller 
estimation  errors  than  either  Ha( 1 ,2,3)  or  Ha(3,3,3)  in  the  presence  of 
outliers. 
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